#include "motor.h"
#include <stdlib.h>
#include "ring.h"
char pid_busy=0,stop_flag=0,zhuan_flag=0;
int pid_flag=2;
int VV;
int FL,FR,BL,BR;
float target_xx,target_yy;
FILE *f2=NULL;
unsigned char ABCD[5]={0};
unsigned char erweima[2]={0};
int fd,fd_1,fd_3;
int color_num=0,color_re=0;
int color_blue,color_red,color_green;
int color_max;
char rgb[5]={0};
char bufff[10] = { 0 };
char str;
char buf_python[10] = { 0 };
char laba_baise[6]={0xAA,0x07,0x02,0x00,0x01,0xB4};
int num_x=0,num_y=0,end=0;
//int a,b,c,d,e,f,g,flage=0;
FILE* f;
void motor_init()
{
	pinMode(MOTOR1,OUTPUT);
	pinMode(MOTOR2,OUTPUT);
	pinMode(PWM1,OUTPUT);        
	pinMode(MOTOR3,OUTPUT);
	pinMode(MOTOR4,OUTPUT);
	pinMode(PWM2,OUTPUT);       
	pinMode(MOTOR5,OUTPUT);
	pinMode(MOTOR6,OUTPUT);
	pinMode(PWM3,OUTPUT);        
	pinMode(MOTOR7,OUTPUT);
	pinMode(MOTOR8,OUTPUT);
	pinMode(PWM4,OUTPUT);     
	pinMode(motor_a,OUTPUT);  
	pinMode(motor_b,OUTPUT);  
	pinMode(LED,OUTPUT);  
	
	softPwmCreate (PWM1, 0, 100);
	softPwmCreate (PWM2, 0, 100);
	softPwmCreate (PWM3, 0, 100);
	softPwmCreate (PWM4, 0, 100);
	digitalWrite(motor_a,0);
	digitalWrite(motor_b,0);
	softPwmWrite(PWM1,0);
	softPwmWrite(PWM2,0);
	softPwmWrite(PWM3,0);
	softPwmWrite(PWM4,0);
	
	}
void erweima_init()
{
	fd_3=serialOpen("/dev/ttyAMA3",9600);
	if(fd_3<0)
	{
		printf("erweima open fail!\r\n");
	}
	else
	printf("Time:%d,erweima open success\n",millis());
	
}
void laba(int colour)//1--hong,2--lv,3--lan,4--bai,5--hei,6--1hao,7--2hao,8--3hao,9--4hao,10--5hao
{
	laba_baise[4]=colour;
	laba_baise[5]=colour+0xB3;
	for(int i=0;i<11;i++)
	{
	serialPutchar(fd_3,laba_baise[i]);
	}
}
char read_erweima()
{
	char s;
	int aa,bb;
	//serialPuts(fd_3,"hhh");
	if(serialDataAvail(fd_3)>0)
	{
	for(int i=0;i<2;i++)
	{
		s=serialGetchar(fd_3);
		if(s=='d')
		{
			break;
			serialFlush (fd_3);
		}
		erweima[i]=(int)s;
		
	}
	serialFlush (fd_3);
		//~ delay(5000);
	//~ printf("rgb[0]=%d\n",erweima[0]);
	//~ printf("rgb[1]=%d\n",erweima[1]);
	if(erweima[1]==13)
	{
		str=erweima[0]-48;
	}
	else
	{
		str=erweima[1]-38;
	}
	//~ printf("%d\n",str);
		return s;
	}
	else return -1;
}
void fang()
{
	digitalWrite(motor_a,0);
	digitalWrite(motor_b,1);
	delay(2000);
	digitalWrite(motor_a,0);
	digitalWrite(motor_b,0);
}

void zhua()
{
	digitalWrite(motor_a,1);
	digitalWrite(motor_b,0);
	delay(2600);
	digitalWrite(motor_a,0);
	digitalWrite(motor_b,0);
}

void front()						//让电机正反转
{
	digitalWrite(MOTOR1, 1);
	digitalWrite(MOTOR2, 0);
	digitalWrite(MOTOR3, 1);
	digitalWrite(MOTOR4, 0);
	digitalWrite(MOTOR5, 1);
	digitalWrite(MOTOR6, 0);
	digitalWrite(MOTOR7, 1);
	digitalWrite(MOTOR8, 0);
}

void back()
{
	digitalWrite(MOTOR1, 0);
	digitalWrite(MOTOR2, 1);
	digitalWrite(MOTOR3, 0);
	digitalWrite(MOTOR4, 1);
	digitalWrite(MOTOR5, 0);
	digitalWrite(MOTOR6, 1);
	digitalWrite(MOTOR7, 0);
	digitalWrite(MOTOR8, 1);
}

void left()						//让电机正反转
{
	digitalWrite(MOTOR1, 0);
	digitalWrite(MOTOR2, 1);
	digitalWrite(MOTOR3, 1);
	digitalWrite(MOTOR4, 0);
	digitalWrite(MOTOR5, 0);
	digitalWrite(MOTOR6, 1);
	digitalWrite(MOTOR7, 1);
	digitalWrite(MOTOR8, 0);
}

void right()						//让电机正反转
{
	digitalWrite(MOTOR1, 1);
	digitalWrite(MOTOR2, 0);
	digitalWrite(MOTOR3, 0);
	digitalWrite(MOTOR4, 1);
	digitalWrite(MOTOR5, 1);
	digitalWrite(MOTOR6, 0);
	digitalWrite(MOTOR7, 0);
	digitalWrite(MOTOR8, 1);
}

void turn()
{
	digitalWrite(MOTOR1, 1);
	digitalWrite(MOTOR2, 0);
	digitalWrite(MOTOR3, 0);
	digitalWrite(MOTOR4, 1);
	digitalWrite(MOTOR5, 0);
	digitalWrite(MOTOR6, 1);
	digitalWrite(MOTOR7, 1);
	digitalWrite(MOTOR8, 0);
}



void move_1(int a,int speed)
{
	digitalWrite(MOTOR1, a);
	digitalWrite(MOTOR2, !a);
	softPwmWrite(PWM1,speed);
}

void move_2(int a,int speed)
{
	digitalWrite(MOTOR3, a);
	digitalWrite(MOTOR4, !a);
	softPwmWrite(PWM2,speed);
}

void move_3(int a,int speed)
{
	digitalWrite(MOTOR5, a);
	digitalWrite(MOTOR6, !a);
	softPwmWrite(PWM3,speed);
}

void move_4(int a,int speed)
{
	digitalWrite(MOTOR7, a);
	digitalWrite(MOTOR8, !a);
	softPwmWrite(PWM4,speed);
}

void stop_1()
{
	digitalWrite(MOTOR1, 1);
	digitalWrite(MOTOR2, 1);
	softPwmWrite(PWM1,0);
}

void stop_2()
{
	digitalWrite(MOTOR3, 1);
	digitalWrite(MOTOR4, 1);
	softPwmWrite(PWM2,0);
}

void stop_3()
{
	digitalWrite(MOTOR5, 1);
	digitalWrite(MOTOR6, 1);
	softPwmWrite(PWM3,0);
}

void stop_4()
{
	digitalWrite(MOTOR7, 1);
	digitalWrite(MOTOR8, 1);
	softPwmWrite(PWM4,0);
}

void stop()						//让电机正反转
{
	digitalWrite(MOTOR1, 1);
	digitalWrite(MOTOR2, 1);
	digitalWrite(MOTOR3, 1);
	digitalWrite(MOTOR4, 1);
	digitalWrite(MOTOR5, 1);
	digitalWrite(MOTOR6, 1);
	digitalWrite(MOTOR7, 1);
	digitalWrite(MOTOR8, 1);
}

void zhuan()
{
	digitalWrite(MOTOR1, 1);
	digitalWrite(MOTOR2, 0);
	digitalWrite(MOTOR3, 0);
	digitalWrite(MOTOR4, 1);
	digitalWrite(MOTOR5, 0);
	digitalWrite(MOTOR6, 1);
	digitalWrite(MOTOR7, 1);
	digitalWrite(MOTOR8, 0);
	softPwmWrite(PWM1,13);
	softPwmWrite(PWM2,13);
	softPwmWrite(PWM3,13);
	softPwmWrite(PWM4,13);
}


char tuoluoyi_init(int flag)
{
	unsigned int tim,aa=0,bb=0;
	pid_flag=4;
	move_2(0,20);
	move_4(0,20);
	//~ shibie();
	while(1)
	{
		//~ if(serialDataAvail(fd_3)>0)
			//~ {
			//~ str=serialGetchar(fd_3);
			//~ //printf("%c\n",str);
			//~ if(str>0&&str<17)
			//~ shibie();
			//~ }
		if(	digitalRead(24)==1)
		{
			aa++;
		}
		if(	digitalRead(21)==1)
		{
			bb++;
		}
		if(aa>=1&&bb>=1)
		{
			stop();
			break;
		}
		delay(10);
	}
	pid_flag=0;
	//~ while((digitalRead(24)==0)&&(digitalRead(21)==0))
	//~ {
			//~ if(serialDataAvail(fd_3)>0)
			//~ {
			//~ str=serialGetchar(fd_3);
			//~ //printf("%c\n",str);
			//~ if(str>0&&str<17)
			//~ shibie();
			//~ }
	//~ printf("%d %d\n",digitalRead(24),digitalRead(21));
	//~ delay(10);
	//~ }
	//~ stop();
		if(flag==1)
		{
		move_x_y_xiang(dir_x,-50,17,0);
		return 0;
		}
		tim=millis();
		//~ while(digitalRead(24)==0)
		//~ {
			//if(millis()-tim>1500)
			//break;
		//~ move_2(0,20);
		//~ }
		//~ stop_2();
		tim=millis();
		//~ while(digitalRead(21)==0)
		//~ {
			//if(millis()-tim>1500)
		//	break;
			move_4(0,20);
		
		//~ }
		//~ stop_4();
	tim=millis();
	while(1)
	{
		//~ if(serialDataAvail(fd_3)>0)
		//~ {
			//~ str=serialGetchar(fd_3);
			//~ //printf("%c\n",str);
			//~ if(str>0&&str<17)
			//~ shibie();
		//~ }
		if(digitalRead(24)==1)
		{
		move_2(0,33);
		delay(30);
		stop_2();
		}
		else
		{
		move_2(1,33);
		delay(30);
		stop_2();
		}
		
		if(digitalRead(21)==1)
		{
		move_4(0,33);
		delay(40);
		stop_4();
		}
		else
		{
		move_4(1,33);
		delay(40);
		stop_4();
		}
		delay(10);
		if(millis()-tim>3000)
		break;
		
	}
	
	while(1)
	{
		//~ if(serialDataAvail(fd_3)>0)
			//~ {
			//~ str=serialGetchar(fd_3);
			//~ //printf("%c\n",str);
			//~ if(str>0&&str<17)
			//~ shibie();
			//~ }
		if(digitalRead(24)==1)
		{
		move_2(0,20);
		delay(20);
		stop_2();
		}
		else
		{
		move_2(1,20);
		delay(20);
		stop_2();
		}
		
		if(digitalRead(21)==1)
		{
		move_4(0,20);
		delay(20);
		stop_4();
		}
		else
		{
		move_4(1,20);
		delay(20);
		stop_4();
		}
		delay(30);
		if(millis()-tim>8000)
		break;
	}
	//~ }
		//~ else
		//~ {
		//~ move_4(0,15);
		//~ delay(30);
		//~ stop_4();
		//~ }
		delay(50);
	
}


void shibie()
{
		int y=0;
		
		/*
		while(1)
		{
			if(serialDataAvail(fd_3)>0)
			{
			str=serialGetchar(fd_3);
			printf("%c\n",str);
			if(str-'0'>0&&str-'0'<17)break;
			}
			//~ p = fopen("a.txt","r");
			//~ //fgets(str, LENGTH, p);
			//~ fscanf(p, "%s", buff);
			//~ if(strcmp(buff,"1")==0)			c=1;
			//~ else if(strcmp(buff,"2")==0)	c=2;
			//~ else if(strcmp(buff,"3")==0)	c=3;
			//~ else if(strcmp(buff,"4")==0)	c=4;
			//~ else if(strcmp(buff,"5")==0)	c=5;
			//~ else if(strcmp(buff,"6")==0)	c=6;
			//~ else if(strcmp(buff,"7")==0)	c=7;
			//~ else if(strcmp(buff,"8")==0)	c=8;
			//~ else if(strcmp(buff,"9")==0)	c=9;
			//~ else if(strcmp(buff,"10")==0)	c=10;
			//~ else if(strcmp(buff,"11")==0)	c=11;
			//~ else if(strcmp(buff,"12")==0)	c=12;
			//~ else if(strcmp(buff,"13")==0)	c=13;
			//~ else if(strcmp(buff,"14")==0)	c=14;
			//~ else if(strcmp(buff,"15")==0)	c=15;
			//~ else if(strcmp(buff,"16")==0)	c=16;
			//~ else c=0;
			//~ if (c!=0)break;
			
		}
		* */
		//~ if(serialDataAvail(fd_3)>0)
		//~ {
			//~ str=serialGetchar(fd_3);
			//~ //printf("%c\n",str);
			//~ if(str>0&&str<17)
			//~ shibie();
		//~ }
		switch (str)
        {
            case 1:
            ABCD[0]=5;ABCD[1]=4;ABCD[2]=1;ABCD[3]=2;ABCD[4]=3;
            break;

            case 2:
            ABCD[0]=4;ABCD[1]=5;ABCD[2]=1;ABCD[3]=2;ABCD[4]=3;
            break;

            case 3:
            ABCD[0]=4;ABCD[1]=5;ABCD[2]=2;ABCD[3]=1;ABCD[4]=3;
            break;

            case 4:
            ABCD[0]=3;ABCD[1]=4;ABCD[2]=5;ABCD[3]=1;ABCD[4]=2;
            break;

            case 5:
            ABCD[0]=2;ABCD[1]=3;ABCD[2]=5;ABCD[3]=1;ABCD[4]=4;
            break;

            case 6:
            ABCD[0]=4;ABCD[1]=1;ABCD[2]=3;ABCD[3]=5;ABCD[4]=2;
            break;

            case 7:
            ABCD[0]=3;ABCD[1]=2;ABCD[2]=5;ABCD[3]=4;ABCD[4]=1;
            break;

            case 8:
            ABCD[0]=2;ABCD[1]=4;ABCD[2]=3;ABCD[3]=5;ABCD[4]=1;
            break;

            case 9:
            ABCD[0]=4;ABCD[1]=2;ABCD[2]=5;ABCD[3]=3;ABCD[4]=1;
            break;

            case 10:
            ABCD[0]=5;ABCD[1]=1;ABCD[2]=3;ABCD[3]=2;ABCD[4]=4;
            break;

            case 11:
            ABCD[0]=1;ABCD[1]=3;ABCD[2]=2;ABCD[3]=5;ABCD[4]=4;
            break;

            case 12:
            ABCD[0]=2;ABCD[1]=1;ABCD[2]=5;ABCD[3]=3;ABCD[4]=4;
            break;

            case 13:
            ABCD[0]=4;ABCD[1]=1;ABCD[2]=3;ABCD[3]=2;ABCD[4]=5;
            break;
            
            case 14:
            ABCD[0]=1;ABCD[1]=2;ABCD[2]=4;ABCD[3]=3;ABCD[4]=5;
            break;

            case 15:
            ABCD[0]=3;ABCD[1]=4;ABCD[2]=2;ABCD[3]=1;ABCD[4]=5;
            break;

            case 16:
            ABCD[0]=2;ABCD[1]=3;ABCD[2]=1;ABCD[3]=4;ABCD[4]=5;
            break;

            default:
            break;
		}
		if(ABCD[0]!=0)
		{
			for(y=0;y<5;y++)
			{
				printf("%d ",ABCD[y]);
				}
			printf("\n");
		}
		//fclose(p);
		//delay(1000);
     
	}
	

void shibie2()
{
		int y=0;
		
		/*
		while(1)
		{
			if(serialDataAvail(fd_3)>0)
			{
			str=serialGetchar(fd_3);
			printf("%c\n",str);
			if(str-'0'>0&&str-'0'<17)break;
			}
			//~ p = fopen("a.txt","r");
			//~ //fgets(str, LENGTH, p);
			//~ fscanf(p, "%s", buff);
			//~ if(strcmp(buff,"1")==0)			c=1;
			//~ else if(strcmp(buff,"2")==0)	c=2;
			//~ else if(strcmp(buff,"3")==0)	c=3;
			//~ else if(strcmp(buff,"4")==0)	c=4;
			//~ else if(strcmp(buff,"5")==0)	c=5;
			//~ else if(strcmp(buff,"6")==0)	c=6;
			//~ else if(strcmp(buff,"7")==0)	c=7;
			//~ else if(strcmp(buff,"8")==0)	c=8;
			//~ else if(strcmp(buff,"9")==0)	c=9;
			//~ else if(strcmp(buff,"10")==0)	c=10;
			//~ else if(strcmp(buff,"11")==0)	c=11;
			//~ else if(strcmp(buff,"12")==0)	c=12;
			//~ else if(strcmp(buff,"13")==0)	c=13;
			//~ else if(strcmp(buff,"14")==0)	c=14;
			//~ else if(strcmp(buff,"15")==0)	c=15;
			//~ else if(strcmp(buff,"16")==0)	c=16;
			//~ else c=0;
			//~ if (c!=0)break;
			
		}
		* */
		//~ if(serialDataAvail(fd_3)>0)
		//~ {
			//~ str=serialGetchar(fd_3);
			//~ //printf("%c\n",str);
			//~ if(str>0&&str<17)
			//~ shibie();
		//~ }
		switch (str)
        {
            case 1:
            ABCD[0]=1;ABCD[1]=2;ABCD[2]=3;ABCD[3]=0;
            break;

            case 2:
            ABCD[0]=1;ABCD[1]=3;ABCD[2]=2;ABCD[3]=0;
            break;

            case 3:
            ABCD[0]=2;ABCD[1]=1;ABCD[2]=3;ABCD[3]=0;
            break;

            case 4:
            ABCD[0]=2;ABCD[1]=3;ABCD[2]=1;ABCD[3]=0;
            break;

            case 5:
            ABCD[0]=3;ABCD[1]=1;ABCD[2]=2;ABCD[3]=0;
            break;

            case 6:
            ABCD[0]=3;ABCD[1]=2;ABCD[2]=1;ABCD[3]=0;
            break;

            

            default:
            break;
		}
		if(ABCD[4]==0)
		{
			for(y=0;y<3;y++)
			{
				printf("%d ",ABCD[y]);
				}
			printf("\n");
		}
		//fclose(p);
		//delay(1000);
     
	}

void duoji_init()
{
	fd_1=serialOpen("/dev/ttyAMA1",115200);
	if(fd_1<0)
	{
		printf("duoji open fail!\r\n");
	}
	else
	printf("Time:%d,舵机启动成功\n",millis());
	}
void set_angle(unsigned char ID,int agl,unsigned int tim)
{
	unsigned char pos1,pos2,interval1,interval2,Usum;
	unsigned char f;
	
	if (agl>=0)
	{
        pos1 = agl%256;
        pos2 = agl/256;
	}
	if (agl<0)
	{
        pos1 = (0-agl)%256;
        pos2 = (0-agl)/256;
        pos1 = 255-pos1;
        pos2 = 255-pos2;
	}
	interval1 = tim%256;
	interval2 = tim/256;
	
	Usum = (0x12+0x4c+0x08+0x07+ID+pos1+pos2+interval1+interval2)%256;
	
	unsigned char byte[12]={0x12,0x4c,0x08,0x07,ID,pos1,pos2,interval1,interval2,0,0,Usum};
	for(f=0;f<12;f++)
	{serialPutchar(fd_1,byte[f]);}
}
void jixiebi_down()
{
	set_angle(1,-480,1000);
	}
void jixiebi_down2()
{
	set_angle(1,-650,1000);
	}
void jixiebi_up()
{
	set_angle(1,-1330,800);
	}
void jixiebi_medium()
{
	set_angle(4,-100,600);
	}
void jixiebi_medium2()
{
	set_angle(1,-200,600);
	}
	
void shexiangtou_out()
{
	set_angle(3,430,2000);
	}
void shexiangtou_in()
{
	set_angle(3,-1340,2000);
	}
void dangban_fang()
{
	set_angle(4,390,800);
}
void dangban_down()
{
	set_angle(4,-510,800);
}
void dangban_down1()
{
	set_angle(1,-1330,800);
	set_angle(4,-510,800);
}

void turn_wheel_y()
{
	set_angle(6,-150,900);//x
	set_angle(8,1000,800);//y
	delay(1000);
	set_angle(7,-620,900);//x
	set_angle(9,-700,900);//y
}

void turn_wheel_x()
{
	set_angle(6,-1100,900);//x
	set_angle(8,0,900);//y
	delay(1000);
	set_angle(7,380,900);//x
	set_angle(9,260,900);//y
}

void get_pix()
{

		
	f = fopen("ring.txt", "r");
	if (f != NULL)
    {
		
		fscanf(f, "%s", bufff);
		//printf("%s\n",&bufff);
		num_x=num_y=0;
		for(int i=0;i<8;i++)
		{
			end=1;
			//printf("%c ",bufff[i]);
			if(bufff[i]=='+')
			{
				for(int j=0;j<i;j++)
				{
					//printf("dan= %d ",bufff[j]-'0');
					num_x+=(bufff[j]-'0')*pow(10,i-j-1);
					}
				//~ printf("num_x= %d ",num_x);
				for(int j=i+1;j<8;j++)
				{
					//printf("dan= %d ",bufff[j]-'0');
					if(bufff[j]==0)
					{
						for(int k=i+1;k<j;k++)
						{
						num_y+=(bufff[k]-'0')*pow(10,j-k-1);
						}
						//~ printf("num_y= %d ",num_y);
						
						}
					end=0;
					}
				
			}
			}
			//~ printf("\n");
		}
		fclose(f);
		
	}
int turn_angle(int x)
{
	int len,f;
	switch (x)
	{
		case 3:
		set_angle(2,-270,1000);				//数值减小，转逆时针转动
		break;
		case 2:
		set_angle(2,-970,1000);
		break;
		case 1:
		set_angle(2,-1700,1000);
		break;
		case 4:
		set_angle(2,440,1000);
		break;
		case 5:
		set_angle(2,1180,1000);
		break;
		default:
		return 0;
			break;
		}
	}
	
void move_x_y(char dir,float target,int speed ,char lo)
{
	float tem_x=x_pos;
	float tem_y=y_pos;
	if(dir)
	{
		if(pid_flag==2)
		{
			target_xx=x_pos;
			turn_wheel_y();
			delay(1000);
			pid_flag=1;
		}
		while(pid_busy);
		if(target-tem_y>=0)front();
		else back();
		while(fabs(y_pos-target)>5)
		{
			if(fabs(y_pos-target)<5)
			break;
			pid_flag=1;
			float a,b;
			if(fabs(x_pos_tem-target_xx)>5)
			{
				a=0.5;
				b=1;
			}
			else
			{
				a=1;
				b=0.5;
			}
			if(target-tem_y>=0)
			{
				softPwmWrite(PWM1,speed+PID_OUT_angle*a-PID_OUT_pos*b);
				softPwmWrite(PWM4,speed+PID_OUT_angle*a-PID_OUT_pos*b);
				softPwmWrite(PWM3,speed-PID_OUT_angle*a+PID_OUT_pos*b);
				softPwmWrite(PWM2,speed-PID_OUT_angle*a+PID_OUT_pos*b);
				//~ printf("PID_OUT_pos=%f\n",PID_OUT_pos);
				//~ printf("target_xx=%f\n",target_xx);
				//~ printf("x_pos=%f\n",x_pos);
				//~ printf("y_pos=%f\n",y_pos);
				//~ printf("angle=%f\n",angle);
				//~ printf("\n");
			}
			else
			{
				softPwmWrite(PWM3,speed+PID_OUT_angle*a+PID_OUT_pos*b);
				softPwmWrite(PWM2,speed+PID_OUT_angle*a+PID_OUT_pos*b);
				softPwmWrite(PWM1,speed-PID_OUT_angle*a-PID_OUT_pos*b);
				softPwmWrite(PWM4,speed-PID_OUT_angle*a-PID_OUT_pos*b);
			}
			
	   		
	   		//~ printf("PID_OUT_angle=%f\n",PID_OUT_angle);
			//~ printf("PID_OUT_pos=%f\n",PID_OUT_pos);
	   		//~ printf("c=%f\n",speed+PID_OUT_angle*a-PID_OUT_pos*x);
	   		//~ printf("d=%f\n",speed-PID_OUT_angle*a+PID_OUT_pos*x);
	   		//~ printf("e=%f\n",e);
	   		//~ printf("x_pos=%f\n",x_pos);
			//~ printf("y_pos=%f\n",y_pos);
			//~ printf("angle=%f\n",angle);
			//~ printf("\n");
			}
		while(pid_busy);
		stop();
	}
	else
	{
		if(pid_flag==1)
		{
			target_yy=y_pos;
			turn_wheel_x();
			delay(1000);
		}
		while(pid_busy);
		if(target-tem_x<=0)left();
		else right();
		while(fabs(x_pos-target)>5)
		{
			pid_flag=2;
			float a,b;
			if(fabs(y_pos-target_yy)>10)
			{
				a=1;
				b=1.2;
			}
			else
			{
				a=1;
				b=1;
			}
			if(target-tem_x>=0)
			{
				softPwmWrite(PWM3,speed-PID_OUT_angle*a-PID_OUT_pos*b);
				softPwmWrite(PWM4,speed-PID_OUT_angle*a-PID_OUT_pos*b);
				softPwmWrite(PWM1,speed+PID_OUT_angle*a+PID_OUT_pos*b);
				softPwmWrite(PWM2,speed+PID_OUT_angle*a+PID_OUT_pos*b);
			}
			else
			{
				softPwmWrite(PWM1,speed-PID_OUT_angle*a+PID_OUT_pos*b);
				softPwmWrite(PWM2,speed-PID_OUT_angle*a+PID_OUT_pos*b);
				softPwmWrite(PWM3,speed+PID_OUT_angle*a-PID_OUT_pos*b);
				softPwmWrite(PWM4,speed+PID_OUT_angle*a-PID_OUT_pos*b);
			}
			
			//~ printf("PID_OUT_pos=%f\n",PID_OUT_pos);
			//~ printf("target_xx=%f\n",target_xx);
			//~ printf("x_pos=%f\n",x_pos);
			//~ printf("y_pos=%f\n",y_pos);
			//~ printf("angle=%f\n",angle);
			//~ printf("\n");
			}
		while(pid_busy);
		stop();
	}
}

void move_x_y_xiang(char dir,float target,int speed,char fg)
{
	float tem_x=x_pos;
	float tem_y=y_pos;
	if(dir)
	{
		if(pid_flag==2)
		{
			target_xx=x_pos;
			turn_wheel_y();
			delay(1500);
			turn_wheel_x();
			delay(1500);
			turn_wheel_y();
			delay(1000);
		}
		while(pid_busy);
		if(target>=0)front();
		else back();
		while(fabs(y_pos-tem_y)<=fabs(target))
		{
			pid_flag=1;
			float a,b;
			if(fabs(x_pos_tem)>5)
			{
				a=0.5;
				b=1;
			}
			else
			{
				a=1;
				b=0.5;
			}
			if(target>=0)
			{
				softPwmWrite(PWM1,speed+PID_OUT_angle*a-PID_OUT_pos*b);
				softPwmWrite(PWM4,speed+PID_OUT_angle*a-PID_OUT_pos*b);
				softPwmWrite(PWM3,speed-PID_OUT_angle*a+PID_OUT_pos*b);
				softPwmWrite(PWM2,speed-PID_OUT_angle*a+PID_OUT_pos*b);
				//~ printf("PID_OUT_pos=%f\n",PID_OUT_pos);
				//~ printf("target_xx=%f\n",target_xx);
				//~ printf("x_pos=%f\n",x_pos);
				//~ printf("y_pos=%f\n",y_pos);
				//~ printf("angle=%f\n",angle);
				//~ printf("\n");
				delay(40);
			}
			else
			{
				softPwmWrite(PWM3,speed+PID_OUT_angle*a-PID_OUT_pos*b);
				softPwmWrite(PWM2,speed+PID_OUT_angle*a-PID_OUT_pos*b);
				softPwmWrite(PWM1,speed-PID_OUT_angle*a+PID_OUT_pos*b);
				softPwmWrite(PWM4,speed-PID_OUT_angle*a+PID_OUT_pos*b);
			}
		}
			
		while(pid_busy);
		stop();
	}
	else
	{
		if(pid_flag==1)
		{
			target_yy=y_pos;
			turn_wheel_x();
			delay(1500);
			turn_wheel_y();
			delay(1500);
			turn_wheel_x();
			delay(1500);
		}
		pid_flag=2;
		while(pid_busy);
		if(target<=0)left();
		else right();
		while(fabs(x_pos-tem_x)<=fabs(target))
		{
			pid_flag=1;
			float a,b;
			if(fabs(y_pos_tem)>5)
			{
				a=0.5;
				b=1;
			}
			else
			{
				a=1;
				b=0.5;
			}
			if(target>=0)
			{
				softPwmWrite(PWM3,speed-PID_OUT_angle*a-PID_OUT_pos*b);
				softPwmWrite(PWM4,speed-PID_OUT_angle*a-PID_OUT_pos*b);
				softPwmWrite(PWM1,speed+PID_OUT_angle*a+PID_OUT_pos*b);
				softPwmWrite(PWM2,speed+PID_OUT_angle*a+PID_OUT_pos*b);
				//~ printf("PID_OUT_pos=%f\n",PID_OUT_pos);
				//~ printf("target_xx=%f\n",target_xx);
				//~ printf("x_pos=%f\n",x_pos);
				//~ printf("y_pos=%f\n",y_pos);
				//~ printf("angle=%f\n",angle);
				//~ printf("\n");
				delay(40);
			}
			else
			{
				softPwmWrite(PWM1,speed-PID_OUT_angle*a+PID_OUT_pos*b);
				softPwmWrite(PWM2,speed-PID_OUT_angle*a+PID_OUT_pos*b);
				softPwmWrite(PWM3,speed+PID_OUT_angle*a-PID_OUT_pos*b);
				softPwmWrite(PWM4,speed+PID_OUT_angle*a-PID_OUT_pos*b);
			}
		}
			
		while(pid_busy);
		stop();
	}
}
void ma()
{
		color_num++;
		//printf("hello");
}
void blue()
{
	digitalWrite(s2,0);
	digitalWrite(s3,1);
	} 
void clean()
{
	digitalWrite(s2,0);
	digitalWrite(s3,1);
	} 
void red()
{
	digitalWrite(s2,0);
	digitalWrite(s3,0);
	} 
	
void green()
{
	digitalWrite(s2,1);
	digitalWrite(s3,1);
	} 
void color_init()
{
	pthread_create(&t_color,NULL,color,NULL);
	//pthread_detach(t4);
	}
void *ring_py(void *args)
{
	system("python3 ring.py");
	system("python3 ring.py");
	system("python3 ring.py");
	pthread_exit(NULL);
	}
//~ void jiema_init()
//~ {
	//~ pthread_t jima_t;
	//~ pthread_create(&jima_t,NULL,jiema,NULL);
	//~ }
//~ void *jiema(void *args)
//~ {
	 
	 //~ while(1)
	 //~ {
		
		//~ FILE *pf = fopen("ring.txt", "r");
		//~ fgets(buf_python, 10, pf);
		//~ for(int i=0;i<8;i++)
		//~ {
			//~ if(buf_python[i]=='+')
			//~ {
				//~ y=atoi(&buf_python[i+1]);
				//~ for (; i <= 9; i++)
				//~ {
					//~ buf_python[i]=0;
				//~ }
				//~ //printf("%s\n", buf_python);
				//~ x=atoi(buf_python);
			//~ }
		//~ }
		//~ //printf("x=%d y=%d\n", x,y);
		//~ fclose(pf);
	//~ }
	//~ }
void find_ring()
{
	pthread_t ring_t;
	pthread_create(&ring_t,NULL,ring_py,NULL);
	pthread_detach(ring_t);
	}

void *color(void *args)
{
	unsigned int tim,t=0,i=0,red1=0,green1=0,blue1=0,white1=0,black1=0,color_max1=0;
	int aa[3]={0};
	float red_p1=0,green_p1=0,blue_p1=0,red_p2=0,green_p2=0,blue_p2=0,red_p,green_p,blue_p;
	wiringPiSetup();
	pinMode(out,INPUT);
	pinMode(s2,OUTPUT);
	pinMode(s3,OUTPUT);
	pullUpDnControl (out,PUD_UP);//涓婃媺杈撳叆
	wiringPiISR(out,INT_EDGE_BOTH,&ma);
	red();
	while(1)
	{
		if(stop_flag==1)pthread_exit(NULL);
		if(millis()-tim>=interval)
		{
			//t++;
			
			while(t>3)
			{
				color_blue=color_blue;
				red_p1=color_red*1.0/color_green;
				red_p2=color_red*1.0/color_blue;
				green_p1=color_green*1.0/color_red;
				green_p2=color_green*1.0/color_blue;
				blue_p1=color_blue*1.0/color_green;
				blue_p2=color_blue*1.0/color_red;
				red_p=red_p1+red_p2;
				green_p=green_p1+green_p2;
				blue_p=blue_p1+blue_p2;
				//~ printf("\n");
				//~ printf("red : %0.3f %0.3f %0.3f %d\n",red_p1,red_p2,red_p,color_red);
				//~ printf("green : %0.3f %0.3f %0.3f %d\n",green_p1,green_p2,green_p,color_green);
				//~ printf("blue : %0.3f %0.3f %0.3f %d\n",blue_p1,blue_p2,blue_p,color_blue);
				//~ printf("color_max=%d\n",color_max);
				
				if(color_green>2000&&color_red>2000&&color_blue>1000)
				{
					color_max1=4;
					t=0;
					goto end;
					}
				if(color_red<350&&color_green<350&&color_blue<350)
				{
					color_max1=5;
					t=0;
					goto end;
					}
					
				//~ if((color_red>color_green)&&(color_red>color_blue))
				//~ {
					//~ color_max=1;
					//~ }
				//~ if((color_green>color_red)&&(color_green>color_blue))
				//~ {
					//~ color_max=2;
					//~ }
				//~ if((color_blue>color_green)&&(color_blue>color_red))
				//~ {
					//~ color_max=3;
					//~ }
					
				if((red_p>green_p)&&(red_p>blue_p))
				{
					color_max1=1;
					}
				if((green_p>red_p)&&(green_p>blue_p))
				{
					color_max1=2;
					}
				if((blue_p>red_p)&&(blue_p>green_p))
				{
					color_max1=3;
					}
				end:
				t=0;
				aa[i]=color_max1;
				i++;
				if(i==3)
				{
				i=0;
				for(int j=0;j<3;j++)
				{
					if(aa[j]==1)red1++;
					if(aa[j]==2)green1++;
					if(aa[j]==3)blue1++;
					if(aa[j]==4)white1++;
					if(aa[j]==5)black1++;
				}
				if((red1>green1)&&(red1>blue1)&&(red1>white1)&&(red1>black1))color_max=1;
				if((green1>red1)&&(green1>blue1)&&(green1>white1)&&(green1>black1))color_max=2;
				if((blue1>green1)&&(blue1>red1)&&(blue1>white1)&&(blue1>black1))color_max=3;
				if((white1>green1)&&(white1>blue1)&&(white1>red1)&&(white1>black1))color_max=4;
				if((black1>green1)&&(black1>blue1)&&(black1>white1)&&(black1>red1))color_max=5;
				red1=green1=blue1=white1=black1=0;
				}
				
				
			}
			tim+=interval;
			color_re=color_num;
			if(t==0)
			{
				//red();
				
				color_red=color_re;
				green();
				//printf("red=%d\n",color_re);
			
				}
			if(t==1)
			{
				
				
				color_green=color_re;
				blue();
				//printf("green=%d\n",color_re);
				
				}
			if(t==2)
			{
				
				
				color_blue=color_re;
				red();
				//printf("blue=%d\n",color_re);
				
				}
			color_num=0;
			t++;
			/*
			printf("color_re=%d\n",color_num);
			color_num=0;
			tim+=interval;
			* */
			}
		
		}
	}
char zhuan_color(char x)
{
	for(int i=0;i<5;i++)
	{
		if(rgb[i]==x)
		{
			turn_angle(i+1);
			return 1;
			
			}
		}
	return 0;
		
	}
//~ void fang_init()
//~ {
	//~ pthread_create(&t_fang,NULL,fan,NULL);
	//~ }

